#include <cstdlib>
#include <iostream>
#include <libobsensor/hpp/Error.hpp>
#include <memory>
#include <opencv2/opencv.hpp>
 
#include "camera.h"

// 定义棋盘格的尺寸
int CHECKERBOARD[2]{ 8,8 };

int main(void) {
    try {
        Camera camera(true, false, false);
        camera.start();
        std::string imgname;
        int f = 1;
        while (cv::waitKey(1) != 27) { // ESC的ASCII码是27，按ESC键可以终止程序
            auto frame_set = camera.get();
            if (frame_set != nullptr) {
                cv::Mat img = camera.frame2mat(frame_set->colorFrame());
                if (!img.empty()) {
                    cv::Mat gray;

                    std::vector<cv::Point2f> corner_pts;
                    bool success;

                    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);

                    // Finding checker board corners
                    // 寻找角点
                    // If desired number of corners are found in the image then success = true
                    // 如果在图像中找到所需数量的角，则success = true
                    // opencv4以下版本，flag参数为CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE
                    success = cv::findChessboardCorners(gray, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);

                    /*
                    * If desired number of corner are detected,
                    * we refine the pixel coordinates and display
                    * them on the images of checker board
                    */
                    // 如果检测到所需数量的角点，我们将细化像素坐标并将其显示在棋盘图像上
                    if (success)
                    {
                        // 如果是OpenCV4以下版本，第一个参数为CV_TERMCRIT_EPS | CV_TERMCRIT_ITER
                        cv::TermCriteria criteria(cv::TermCriteria::EPS | cv::TermCriteria::Type::MAX_ITER, 30, 0.001);

                        // refining pixel coordinates for given 2d points.
                        // 为给定的二维点细化像素坐标
                        cv::cornerSubPix(gray, corner_pts, cv::Size(11, 11), cv::Size(-1, -1), criteria);

                        // Displaying the detected corner points on the checker board
                        // 在棋盘上显示检测到的角点
                        cv::drawChessboardCorners(img, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts, success);

                        printf("success!\n");
                    }
                    else
                    {
                        printf("fail!\n");
                    }

                    cv::imshow("source", img);
                    cv::waitKey(10);	//延时30
                }
                img = camera.frame2mat(frame_set->irFrame());
                if (!img.empty()) {
                    cv::imshow("IR", img);
                }

                img = camera.frame2mat(frame_set->depthFrame());
                if (!img.empty()) {
                    cv::imshow("Depth", img);
                }
            }
        }
        camera.stop();
        cv::destroyAllWindows();
    } catch (const ob::Error &e) {
        std::cerr << "Function:" << e.getName() << "\nargs:" << e.getArgs()
                  << "\nmessage:" << e.getMessage() << "\ntype:" << e.getExceptionType()
                  << std::endl;
        exit(EXIT_FAILURE);
    } catch (const std::exception &e) {
        std::cerr << e.what() << std::endl;
        exit(EXIT_FAILURE);
    } catch (...) {
        std::cerr << "Unexpected Error!" << std::endl;
        exit(EXIT_FAILURE);
    }
    return EXIT_SUCCESS;
}